#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream.h>

#include "Object.h"
#include "GL/glut.h"
#include "include.h"
#include "PinnedObject.h"
#include "GrenadeObject.h"
#include "FrictionInfo.h"
#include "List.h"

float abs(float n)
{
	if(n < 0)
		return -1.0 * n;
	return n;
}

Object::Object()
{
	collideWithGround = true;
	frictionInfoList = NULL;
	isColliding = false;
	letMeGo = false;
	objectToCreate = NULL;
	objectToDestroy = NULL;
	collideWhenGrabbed = false;
	material = NULL;
	keyPointList = new Vector2DList();
	objectId = OBJECT_CLASS;
	position = Vector2D(50,50);
	velocity = Vector2D(0,0);
	acceleration = Vector2D(0,0);
	vertices = NULL;//(Vector2D*)malloc(sizeof(Vector2D)*4);
	//assert(vertices != NULL);
	isPinned = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
	CalculateUpVector();
	isOnGround = false;
	hitGround = false;
	onGroundFilter = 0;
	isOnRightWall = false;
	isOnLeftWall = false;
	
	/*vertices[0] = Vector2D(-1,1);
	vertices[1] = Vector2D(1,1);
	vertices[2] = Vector2D(1,-1);
	vertices[3] = Vector2D(-1,-1);
	CalculateTextureVertices(0.0);
	CalculateMomentOfInertia();*/
}
void Object::CalculateMomentOfInertia()
{
	if(numVertices == 0)
	{
		angularMass = mass;
		return;
	}
	
	 angularMass = 0.0;
	 boundingRadius = 0.0;
	 for(int i = 0; i < numVertices; i++)
	 {
	 	float distance = vertices[i].Magnitude();
	 	angularMass +=  distance * distance;
	 	if(distance > boundingRadius)
	 		boundingRadius = distance;
	 }
	 angularMass *= mass/numVertices;
}	
Object::Object( TextureManager * textureManager,
			Vector2D position,
			Vector2D * vertices,
			int numVertices,
			float mass,
			bool isGrabbable)
{
	collideWithGround = true;
	cantShift = false;
	frictionInfoList = NULL;
	isColliding = false;
	letMeGo = false;
	objectToDestroy = NULL;
	objectToCreate = NULL;
	this->collideWhenGrabbed = false;
	this->textureManager = textureManager;
	this->material = new Material("NA",0,0,isGrabbable,0.1,-1);
	keyPointList = new Vector2DList();
	objectId = OBJECT_CLASS;
	this->position = position;
	this->velocity = Vector2D(0,0);
	this->acceleration = Vector2D(0,0);
	this->vertices = vertices;
	this->numVertices = numVertices;
	this->rotation = 0.0;
	this->angularVelocity = 0.0;
	this->angularAcceleration = 0.0;
	this->mass = mass;
	this->isPinned = false;	
	isOnGround = false;
	hitGround = false;
	onGroundFilter = 0;
	isOnRightWall = false;
	isOnLeftWall = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
	CalculateUpVector();
	CalculateMomentOfInertia();
	//printf("Object calculate texture vertices\n");
	CalculateTextureVertices(0.0);
}
Object::Object(  TextureManager * textureManager,
			Material * material,
			float textureRotation,
			Vector2D position, 
			Vector2D velocity,
			Vector2D acceleration,
			Vector2D * vertices,
			int numVertices,
			float rotation,
			float angularVelocity,
			float angularAcceleration,
			float mass,
			bool isPinned,
			bool collideWhenGrabbed)
{
	collideWithGround = true;
	cantShift = false;
	frictionInfoList = NULL;
	isColliding = false;
	letMeGo = false;
	objectToDestroy = NULL;
	objectToCreate = NULL;
	this->collideWhenGrabbed = collideWhenGrabbed;
	this->textureManager = textureManager;
	this->material = material;
	keyPointList = new Vector2DList();
	objectId = OBJECT_CLASS;
	this->position = position;
	this->velocity = velocity;
	this->acceleration = acceleration;
	this->vertices = vertices;
	this->numVertices = numVertices;
	this->rotation = rotation;
	this->angularVelocity = angularVelocity;
	this->angularAcceleration = angularAcceleration;
	this->mass = mass;
	this->isPinned = isPinned;	
	isOnGround = false;
	hitGround = false;
	onGroundFilter = 0;
	isOnRightWall = false;
	isOnLeftWall = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
	CalculateUpVector();
	CalculateMomentOfInertia();
	//printf("Object calculate texture vertices\n");
	CalculateTextureVertices(textureRotation);
}
Object::Object(  TextureManager * textureManager,
			Material * material,
			float textureRotation,
			Vector2D position, 
			Vector2D velocity,
			Vector2D acceleration,
			Vector2D * vertices,
			int numVertices,
			float rotation,
			float angularVelocity,
			float angularAcceleration,
			float mass,
			bool isPinned)
{
	collideWithGround = true;
	//printf("Making Object\n");
	cantShift = false;
	frictionInfoList = NULL;
	isColliding = false;
	letMeGo = false;
	objectToCreate = NULL;
	objectToDestroy = NULL;
	collideWhenGrabbed = isPinned;
	this->textureManager = textureManager;
	this->material = material;
	keyPointList = new Vector2DList();
	objectId = OBJECT_CLASS;
	this->position = position;
	this->velocity = velocity;
	this->acceleration = acceleration;
	this->vertices = vertices;
	this->numVertices = numVertices;
	this->rotation = rotation;
	this->angularVelocity = angularVelocity;
	this->angularAcceleration = angularAcceleration;
	this->mass = mass;
	this->isPinned = isPinned;	
	isOnGround = false;
	hitGround = false;
	onGroundFilter = 0;
	isOnRightWall = false;
	isOnLeftWall = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
	CalculateUpVector();
	CalculateMomentOfInertia();
	//printf("Object2 calculateTextureVertices\n");
	CalculateTextureVertices(textureRotation);
}
void Object::CalculateTextureVertices(float theta)
{
	//printf("Object::CalculateTextureVertices numVertices = %d\n", numVertices);
	textureVertices = NULL;
	if(numVertices <= 0)
		return;
	textureVertices = (Vector2D*)malloc(sizeof(Vector2D)*numVertices);
	assert(textureVertices != NULL);
	
	float sinTheta = sin(theta);
	float cosTheta = cos(theta);
	
	for(int i = 0; i < numVertices; i++)
	{
		float newX = vertices[i].x * cosTheta - vertices[i].y * sinTheta;
		float newY = vertices[i].y * cosTheta + vertices[i].x * sinTheta;
		float size = 0.1;
		if(material)
			size = material->textureSize;
		textureVertices[i].x = newX * size;
		textureVertices[i].y = newY * size;
	}	
}
Object::~Object()
{
	printf("deleting object\n");
	if(vertices)
		delete(vertices);
	vertices = NULL;
	printf("d o 2\n");
	if(keyPointList)
		delete(keyPointList);
	keyPointList = NULL;
	if(frictionInfoList)
		delete(frictionInfoList);
	printf("d o 3\n");
	//if(textureVertices)
	//	delete(textureVertices);
	textureVertices = NULL;
	numVertices = 0;
	printf("done deleting object\n");
}
bool Object::LetMeGo()
{
	return letMeGo;
}
void Object::CalculateUpVector()
{
	upVector = Vector2D(0,1);
}
void Object::AddKeyPoint(Vector2D * point)
{
	keyPointList->Add(point);
}
void Object::ReleasingObject()
{
}
void Object::PickingUpObject()
{
}
void Object::RemoveKeyPoint(Vector2D point)
{
	keyPointList->Remove(point);
}
void Object::RemoveKeyPoint(Vector2D * point)
{
	keyPointList->Remove(point);
}
void Object::DrawCollision()
{
	return;
	//collisionValid=true;
	//collisionPoint = translationalForce;
	if(collisionValid)
	{
		Vector2D velAtPoint = collisionPoint + VelocityAtPoint(collisionPoint);
		glPushMatrix();
		glLoadIdentity();
		glTranslatef(position.x,position.y, 0.0);
		glColor3f(0,1,0);
		glBegin(GL_LINES);
		
		glVertex2f(collisionPoint.x, collisionPoint.y);
		glVertex2f(velAtPoint.x,velAtPoint.y);
		//glVertex2f(collisionEdge[0].x,collisionEdge[0].y);
		//glVertex2f(collisionEdge[1].x,collisionEdge[1].y);
		glEnd();
		glPopMatrix();
	}
}
int Object::IsVertex(Vector2D point)
{
	for(int i =0; i < numVertices; i++)
	{
		if(vertices[i] == point)
			return i;
	}
	return -1;
}
void Object::DrawEntryPoint()
{
	
}
bool Object::PreDrawEntryPoint()
{
	glColor4f(1,1,1,1 );
	return false;
}
void Object::Draw()
{
	if(PreDrawEntryPoint())
	{
		DrawEntryPoint();
		return;
	}
		
	glPushMatrix();
	glLoadIdentity();
	glTranslatef(position.x,position.y, 0.0);
	if(textureManager)
		textureManager->EnableTexture(material->GetTextureIndex());
	glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
	glAlphaFunc( GL_GREATER, 0.01 );
  	glEnable( GL_ALPHA_TEST );
	glEnable(GL_TEXTURE_2D);
	glBegin(GL_POLYGON);
	for(int i=0; i < numVertices; i++)
	{	
		if(textureVertices)
			glTexCoord2f(textureVertices[i].x, textureVertices[i].y);
		glVertex2f(vertices[i].x, vertices[i].y);
	}
	glEnd();
	glPopMatrix();
	glDisable(GL_TEXTURE_2D);
	glDisable(GL_BLEND);
	DrawEntryPoint();
}
void Object::AddDamage(float damage)
{
}
void Object::Rotate(Vector2D radius, Vector2D delta)
{
}
bool Object::PreRotate(float theta)
{
	return false;
}
void Object::Rotate(float theta)
{
	if(PreRotate(theta))
	{
		return;
	}
	
	if(abs(theta) < 0.000001)
		return;
		
	float sinTheta = sin(theta);
	float cosTheta = cos(theta);
	
	for(int i = 0; i < numVertices; i++)
	{
		float newX = vertices[i].x * cosTheta - vertices[i].y * sinTheta;
		float newY = vertices[i].y * cosTheta + vertices[i].x * sinTheta;
		vertices[i].x = newX;
		vertices[i].y = newY;
	}	
	float x = upVector.x * cosTheta - upVector.y * sinTheta;
	float y = upVector.y * cosTheta + upVector.x * sinTheta;
	upVector.x = x;
	upVector.y = y;
	
	if(keyPointList)
	{
		keyPointList->Rotate(sinTheta, cosTheta);
	}

	if(collisionValid)
	{
		x = collisionPoint.x * cosTheta - collisionPoint.y * sinTheta;
		y = collisionPoint.y * cosTheta + collisionPoint.x * sinTheta;
		collisionPoint.x = x;
		collisionPoint.y = y;
	}
}
bool Object::PreThinkEntryPoint(double elapsedTime)
{
	return false;
}
void Object::AddFriction(double elapsedTime)
{
	if(!frictionInfoList)
		return;
		
	frictionInfoList->ApplyFrictionalForce(translationalForce, this, elapsedTime);
}
void Object::Think(double elapsedTime)
{
	//numCollisions = 0;
	AddFriction(elapsedTime);
	
	if(PreThinkEntryPoint(elapsedTime))
	{
		printf("Escaping think\n");
		return;
	}
		
	if(isPinned)
	{
		velocity = Vector2D(0,0);
		return;
	}
	
	if(mass != 0.0)
	{
		acceleration = translationalForce / mass;
	}
	else
	{
		acceleration = Vector2D(0,0);
	}
	
	if(angularMass != 0.0)
	{
		angularAcceleration = torque / angularMass;
	}
	else
	{
		angularAcceleration = 0.0;
	}
		
		velocity = velocity + (acceleration * elapsedTime);
		//if(velocity.Magnitude() > 1000)
		//	velocity = !velocity * 1000.0;

		if((velocity.Magnitude() * elapsedTime) > 0.01)
			position = position + (velocity * elapsedTime);
			
		angularVelocity = angularVelocity + (angularAcceleration * elapsedTime);
		//if(angularVelocity > 10)
		//	angularVelocity = 10;

		ThinkEntryPoint(elapsedTime);
		
		translationalForce = Vector2D(0,0);
		torque = 0.0;
		if(hitGround)
		{
			onGroundFilter = 0;
			isOnGround = true;
		}
		else if(isOnGround)
			onGroundFilter++;
		
		if(onGroundFilter > 5)
			isOnGround = false;
			
		if(frictionInfoList)
		{
			delete(frictionInfoList);
			frictionInfoList = NULL;
		}
			
		hitGround = false;
		isColliding = false;
		isOnRightWall = false;
		isOnLeftWall = false;
		lastElapsedTime = elapsedTime;
}
float IntervalDistance( Vector2D point1, Vector2D point2, Vector2D minVertex, Vector2D maxVertex, float minError, float maxError, Vector2D * finalVertex, float * finalError) 
{
    if ((point1.x) < (point2.x)) 
    {
    	*finalVertex = maxVertex;
    	*finalError = minError;
        return point2.x - point1.y;
    } 
    else 
    {
    	*finalVertex = minVertex;
    	*finalError = maxError;
        return point1.x - point2.y;
    }
}
bool Object::Collide(Object * object2, double elapsedTime)
{
	return Collide(this, object2, elapsedTime);
}
bool Object::CollideWithSegment(Object * object1, Object * object2, float * minDistance, float * minError, Vector2D * finalResult, Vector2D * finalOrigin)
{
	bool didCollide = false;
	float startDistance = *minDistance;
	Vector2D startResult = *finalResult;
	Vector2D startOrigin = *finalOrigin;
	for(int i = 0; i < object1->numVertices; i++)
	{
		LineSegment edge1;
		if(i == object1->numVertices - 1)
			edge1 = LineSegment(object1->vertices[i], object1->vertices[0]);
		else
			edge1 = LineSegment(object1->vertices[i], object1->vertices[i+1]);

		Vector2D result, origin;
		float distance, error;
		bool thisDidCollide = edge1.Collide(object2, &distance, &result, &origin);
		didCollide |= thisDidCollide;
		if(thisDidCollide)
		{
			////printf("Segment %d collided!\n", i);
			if(distance > *minDistance)
			{
				*minDistance = distance;
				//minError = error;
				*finalResult = result;
				*finalOrigin = origin;
			}
		}
		else
		{
			*minDistance = startDistance;
			//minError = error;
			*finalResult = startResult;
			*finalOrigin = startOrigin;
			return false;
		}
	}
	return didCollide;	
}
bool Object::Collide2(Object * object1, Object * object2, double elapsedTime)
{
	//I can't believe I have to do this again....... Fuck....
	if((object1->isPinned && object2->isPinned) || (object1->velocity.Magnitude() == 0.0 && object2->velocity.Magnitude() == 0.0))
		return false;
		
	float distance = object1->position.dist(object2->position);
	float maxDistance = (object1->boundingRadius + object2->boundingRadius);

	if(distance > maxDistance)
		return false;
	
	bool didCollide = false;
	float minDistance = -10000.0;
	float minError = 100000.0;
	Vector2D finalResult;
	Vector2D finalOrigin;
	Object * edgeOwner = object1;
	Object * vertexOwner = object2;
	
	didCollide |= CollideWithSegment(object1, object2, &minDistance, &minError, & finalResult, & finalOrigin);
	float middleMinDistance = minDistance;
	didCollide |= CollideWithSegment(object2, object1, &minDistance, &minError, & finalResult, & finalOrigin);
	
	if(minDistance < middleMinDistance)
	{
		edgeOwner = object2;
		vertexOwner = object1;
	}
	
	if(didCollide)
	{
		
		if(finalResult.Magnitude() == 0.0)
			return true;
		
		Vector2D translationalAxis = !finalResult;
		Vector2D deltaPosition = translationalAxis * minDistance;
		
		if(!vertexOwner->isPinned)
			vertexOwner->position = vertexOwner->position + deltaPosition;
		else
			edgeOwner->position = edgeOwner->position - deltaPosition;
		
		vertexOwner->AddForceObjectSpace(finalResult, finalOrigin);
		Vector2D finalOrigin2 = (finalOrigin + vertexOwner->position) - edgeOwner->position;
		edgeOwner->AddForceObjectSpace(-1.0 * finalResult, finalOrigin2);
		vertexOwner->collisionPoint = finalOrigin;
		vertexOwner->collisionValid = true;
		edgeOwner->collisionPoint = finalOrigin2;
		edgeOwner->collisionValid = true;
		vertexOwner->isPinned = true;
		edgeOwner->isPinned = true;
	}
	return didCollide;
		
}	
Vector2D ClosestPointOnSegment(Vector2D& A, Vector2D& B, Vector2D& P)
{
    Vector2D D = B-A;
    float numer = (P-A).dot(D);
    if (numer <= 0.0f)
        return A;
    float denom = D.dot(D);
    if (numer >= denom)
        return B;
    return A + (numer/denom) * D;
}	

bool Object::CanCollideWith(Object * object)
{
	return true;
}
bool Object::CollideSprite(Sprite * sprite)
{
	float distance = position.dist(sprite->position);
	float maxDistance = (boundingRadius + sprite->boundingRadius);

	if(distance > maxDistance)
		return false;

	float minIntervalDistance = maxDistance;
	float minMoveDistance = maxDistance;
	
	Vector2D finalCollisionPoint;// = ClosestPointOnSegment(finalEdge[0], finalEdge[1], finalVertexWorldSpace);
	Vector2D finalEdgeNormal;// = collisionPoint - finalVertexWorldSpace;
	//loop through the edges of object1, find a orthogonal ray, and project the 
	//vertices of each object onto it. If the projected lines don't overlap, 
	//we found a dividing edge.
	//If they do overlap, check to see if this is the minimum overlap distance.
	//If it is, keep track of the vertices making up the edge
	
	for(int i = 0; i < 4; i++)
	{
		Vector2D edge;
		if(i == 3)
			edge = sprite->GetVertex(0) - sprite->GetVertex(i);
		else
			edge = sprite->GetVertex(i+1) - sprite->GetVertex(i);
			
		edge = Vector2D(-edge.y, edge.x);
		edge = !edge;
		
		float min1,max1;
		float min2,max2;
		Vector2D minVertex1, maxVertex1;
		Vector2D minVertex2, maxVertex2;
		float minError1, maxError1;
		float minError2, maxError2;
		
		ProjectPolygon(edge, &min1, &max1, &minVertex1, &maxVertex1, &minError1, &maxError1);
		sprite->ProjectPolygon(edge, &min2, &max2, &minVertex2, &maxVertex2, &minError2, &maxError2);
		
		Vector2D point1 = Vector2D(min1, max1);
		Vector2D point2 = Vector2D(min2, max2);
		Vector2D possibleVertex;
		float possibleError;
		float intervalDistance = IntervalDistance(point1, point2, maxVertex2, minVertex2, maxError2, minError2, &possibleVertex, &possibleError);
		if(intervalDistance > 0)
		{
			//printf("Leaving top\n\n\n");
			return false;
		}
		//printf("IntervalDistance TOP = %f\n", intervalDistance);
		intervalDistance = abs(intervalDistance);

		if( intervalDistance < minIntervalDistance )
		{
				minIntervalDistance = intervalDistance;
		}
			
	}
	/*for(int i = 0; i < numVertices; i++)
	{
		Vector2D edge;
		if(i == numVertices - 1)
			edge = vertices[0] - vertices[i];
		else
			edge = vertices[i+1] - vertices[i];
			
		edge = Vector2D(-edge.y, edge.x);
		edge = !edge;
		
		float min1,max1;
		float min2,max2;
		Vector2D minVertex1, maxVertex1;
		Vector2D minVertex2, maxVertex2;
		float minError1, maxError1;
		float minError2, maxError2;
		
		ProjectPolygon(edge, &min1, &max1, &minVertex1, &maxVertex1, &minError1, &maxError1);
		sprite->ProjectPolygon(edge, &min2, &max2, &minVertex2, &maxVertex2, &minError2, &maxError2);
		
		Vector2D point1 = Vector2D(min1, max1);
		Vector2D point2 = Vector2D(min2, max2);

		printf("point1 = (%f, %f)\n", min1, max1);
		printf("point2 = (%f, %f)\n", min2, max2);
		Vector2D possibleVertex;
		float possibleError;
		float intervalDistance = IntervalDistance(point1, point2, maxVertex2, minVertex2, maxError2, minError2, &possibleVertex, &possibleError);
		printf("intervalDistance %d = %f\n",i, intervalDistance);
		if(intervalDistance > 0)
		{
			//printf("Leaving top\n\n\n");
			return false;
		}
		//printf("IntervalDistance TOP = %f\n", intervalDistance);
		intervalDistance = abs(intervalDistance);

		if( intervalDistance < minIntervalDistance )
		{
				minIntervalDistance = intervalDistance;
		}
			
	}*/
		
	return true;
	
}
bool Object::Collide(Object * object1, Object * object2, double elapsedTime)
{
	GrenadeObject * grenade = NULL;
	Object * victim = NULL;
	if(object1->objectId == GHOST_OBJECT_CLASS || object2->objectId == GHOST_OBJECT_CLASS)
	{
		return false;
	}
	if(object1->objectId == GRENADE_OBJECT_CLASS)
	{
		grenade = (GrenadeObject *)object1;	
		victim = object2;	
	}
	else if(object2->objectId == GRENADE_OBJECT_CLASS)
	{
		grenade = (GrenadeObject *)object2;
		victim = object1;
	}
	if(grenade && grenade->IsFuseExpired())
	{
		return grenade->ExplosionCollide(victim, elapsedTime);
	}
	//I can't believe I have to do this again....... Fuck....
	if(!object1->CanCollideWith(object2)  || !object2->CanCollideWith(object1))
		return false;
	if((object1->isPinned && object2->isPinned) || 
	   (object1->velocity.Magnitude() == 0.0 && object2->velocity.Magnitude() == 0.0 && 
		object1->angularVelocity == 0.0 && object2->angularVelocity == 0.0))
		return false;
		
	float distance = object1->position.dist(object2->position);
	float maxDistance = (object1->boundingRadius + object2->boundingRadius);

	if(distance > maxDistance)
		return false;

	Vector2D finalEdge[2];
	Vector2D finalVertex; // this is the vertex of one object that collides with the edge of another
	Vector2D translationalAxis; 
	float minIntervalDistance = maxDistance;
	float minMoveDistance = maxDistance;
	float minError = 1000000000.0;
	float finalEdgeError = 1000000000.0;
	
	Object * vertexOwner = object2;
	Object * edgeOwner = object1;
	Vector2D finalCollisionPoint;// = ClosestPointOnSegment(finalEdge[0], finalEdge[1], finalVertexWorldSpace);
	Vector2D finalEdgeNormal;// = collisionPoint - finalVertexWorldSpace;
	//loop through the edges of object1, find a orthogonal ray, and project the 
	//vertices of each object onto it. If the projected lines don't overlap, 
	//we found a dividing edge.
	//If they do overlap, check to see if this is the minimum overlap distance.
	//If it is, keep track of the vertices making up the edge
	for(int i = 0; i < object1->numVertices; i++)
	{
		Vector2D edge;
		if(i == object1->numVertices - 1)
			edge = object1->vertices[0] - object1->vertices[i];
		else
			edge = object1->vertices[i+1] - object1->vertices[i];
			
		edge = Vector2D(-edge.y, edge.x);
		edge = !edge;
		
		float min1,max1;
		float min2,max2;
		Vector2D minVertex1, maxVertex1;
		Vector2D minVertex2, maxVertex2;
		float minError1, maxError1;
		float minError2, maxError2;
		
		object1->ProjectPolygon(edge, &min1, &max1, &minVertex1, &maxVertex1, &minError1, &maxError1);
		object2->ProjectPolygon(edge, &min2, &max2, &minVertex2, &maxVertex2, &minError2, &maxError2);
		
		Vector2D point1 = Vector2D(min1, max1);
		Vector2D point2 = Vector2D(min2, max2);

		Vector2D possibleVertex;
		float possibleError;
		float intervalDistance = IntervalDistance(point1, point2, maxVertex2, minVertex2, maxError2, minError2, &possibleVertex, &possibleError);
		
		if(intervalDistance > 0)
		{
			//printf("Leaving top\n\n\n");
			return false;
		}
		//printf("IntervalDistance TOP = %f\n", intervalDistance);
		intervalDistance = abs(intervalDistance);
		
		Vector2D worldEdge[2];
		if(i == object1->numVertices - 1)
		{
			worldEdge[0] = object1->position + object1->vertices[0];
		}
		else
		{
			worldEdge[0] = object1->position + object1->vertices[i+1];
		}
		worldEdge[1] = object1->position + object1->vertices[i];
		Vector2D worldVertex = /*possibleVertex +*/ object2->position;
		float edgeError = worldVertex.dist(worldEdge[0]) + worldVertex.dist(worldEdge[1]);
		Vector2D vertexWorldSpace = possibleVertex+ object2->position;
		Vector2D collisionPoint = ClosestPointOnSegment(worldEdge[0], worldEdge[1], vertexWorldSpace );
		Vector2D edgeNormal = collisionPoint - vertexWorldSpace;
		float mag = edgeNormal.Magnitude();
		
		if( intervalDistance < minIntervalDistance || 
		    ((intervalDistance - minIntervalDistance < 0.00001) && mag < minMoveDistance))//edgeError < finalEdgeError))
		{
			bool compatibleDirection = true;
			if(0)//mag > 0.0)
			{
				Vector2D edgeNormal2 = !edgeNormal;
				Vector2D velocityAtPoint1 = object1->VelocityAtPoint(possibleVertex + object2->position - object1->position);
				Vector2D velocityAtPoint2 = object2->VelocityAtPoint(possibleVertex);
				float velocityProjection1 = edgeNormal2.dot(velocityAtPoint1);
				float velocityProjection2 = edgeNormal2.dot(velocityAtPoint2);
				
				if( abs(velocityProjection1) + abs(velocityProjection2) > 100)
				{
					if(abs(velocityProjection2) > abs(velocityProjection1))
					{
						compatibleDirection = velocityProjection2 < 0;
					}
					else if(abs(velocityProjection2) < abs(velocityProjection1))
						compatibleDirection = velocityProjection1 > 0;
					else
					{
						compatibleDirection = true;
						//printf("CORNER CASE\n");
					}
				}
			}
				
			if(compatibleDirection)
			{
				//printf("Collision top\n");
				finalCollisionPoint = collisionPoint;
				finalEdgeNormal = edgeNormal;
				finalEdge[0] = worldEdge[0];
				finalEdge[1] = worldEdge[1];
				//printf("final edges TOP= (%f, %f) (%f, %f)\n", finalEdge[0].x,finalEdge[0].y, finalEdge[1].x, finalEdge[1].y);
				
				collisionEdge[0] = finalEdge[0] - object1->position;
				collisionEdge[1] = finalEdge[1] - object1->position;
				//Vector2D worldVertex = vertexWorldSpace;//(possibleVertex + object2->position);
				minIntervalDistance = intervalDistance;
				minMoveDistance = mag;
				minError = possibleError;
				finalEdgeError = edgeError;
				finalVertex = possibleVertex;
				translationalAxis = edge;	
			}
			
		}
			
	}
		
	for(int i = 0; i < object2->numVertices; i++)
	{
		Vector2D edge;
		if(i == object2->numVertices - 1)
			edge = object2->vertices[0] - object2->vertices[i];
		else
			edge = object2->vertices[i+1] - object2->vertices[i];
		
		edge = Vector2D(-edge.y, edge.x);
		edge = !edge;
		
		float min1,max1;
		float min2,max2;
		Vector2D minVertex1, maxVertex1;
		Vector2D minVertex2, maxVertex2;
		float minError1, maxError1;
		float minError2, maxError2;
		
		object1->ProjectPolygon(edge, &min1, &max1, &minVertex1, &maxVertex1, &minError1, &maxError1);
		object2->ProjectPolygon(edge, &min2, &max2, &minVertex2, &maxVertex2, &minError2, &maxError2);
		
		Vector2D point1 = Vector2D(min1, max1);
		Vector2D point2 = Vector2D(min2, max2);
		Vector2D possibleVertex;
		float possibleError;
		float intervalDistance = IntervalDistance(point1, point2, minVertex1, maxVertex1, minError1, maxError1, &possibleVertex, &possibleError);
		
		if(intervalDistance > 0)
		{
			//printf("Leaving bottom\n\n\n");
			return false;
		}

		//printf("intervalDistance BOTTOM = %f\n",intervalDistance);
		//printf("possibleError = %f\n", possibleError);
		//printf("minError = %f\n", minError);
		
		Vector2D worldEdge[2];
		if(i == object2->numVertices - 1)
		{
			worldEdge[0] = object2->position + object2->vertices[0];
		}
		else
		{
			worldEdge[0] = object2->position + object2->vertices[i+1];
		}
		worldEdge[1] = object2->position + object2->vertices[i];
		Vector2D worldVertex = /*possibleVertex +*/ object1->position;
		float edgeError = worldVertex.dist(worldEdge[0]) + worldVertex.dist(worldEdge[1]);
		Vector2D vertexWorldSpace = possibleVertex+ object1->position;
		Vector2D collisionPoint = ClosestPointOnSegment(worldEdge[0], worldEdge[1], vertexWorldSpace );
		Vector2D edgeNormal = collisionPoint - vertexWorldSpace;
		float mag = edgeNormal.Magnitude();
		intervalDistance = abs(intervalDistance);

		if( (intervalDistance< minIntervalDistance) ||
			((intervalDistance - minIntervalDistance < 0.00001) && mag < minMoveDistance))//&& possibleError <= minError && edgeError < finalEdgeError))
		{
			bool compatibleDirection = true;
			if(0)//mag > 0.0)
			{
				Vector2D edgeNormal2 = !edgeNormal;
				Vector2D velocityAtPoint1 = object1->VelocityAtPoint(possibleVertex);
				Vector2D velocityAtPoint2 = object2->VelocityAtPoint(possibleVertex + object1->position - object2->position);
				float velocityProjection1 = edgeNormal2.dot(velocityAtPoint1);
				float velocityProjection2 = edgeNormal2.dot(velocityAtPoint2);
				
				
				if( abs(velocityProjection1) + abs(velocityProjection2) > 100)
				{
					if(abs(velocityProjection1) > abs(velocityProjection2))
					{
						compatibleDirection = velocityProjection1 < 0;
					}
					else if(abs(velocityProjection1) < abs(velocityProjection2))
					{
						compatibleDirection = velocityProjection2 > 0;
					}
					else
					{
						//printf("BOTTOM CORNER CASE\n");
					}
				}
			}
				
			if(compatibleDirection)
			{
				finalCollisionPoint = collisionPoint;
				finalEdgeNormal = edgeNormal;
				finalEdge[0] = worldEdge[0];
				finalEdge[1] = worldEdge[1];
				//printf("finaledges Bottom= (%f, %f) (%f, %f)\n", finalEdge[0].x,finalEdge[0].y, finalEdge[1].x, finalEdge[1].y);
				
				object2->collisionEdge[0] = finalEdge[0] - object2->position;
				object2->collisionEdge[1] = finalEdge[1] - object2->position;
				
				minIntervalDistance = intervalDistance;
				minMoveDistance = mag;
				minError = possibleError;
				finalEdgeError = edgeError;
				finalVertex = possibleVertex;	
				vertexOwner = object1;
				edgeOwner = object2;
				translationalAxis = edge;
			}
		}
			
	}

	//printf("FINAL mag = %f\n", minMoveDistance);
	Vector2D finalVertex2 = (finalVertex + vertexOwner->position) - edgeOwner->position;
	Vector2D velocityAtPoint1 = vertexOwner->VelocityAtPoint(finalVertex);
	Vector2D velocityAtPoint2 = edgeOwner->VelocityAtPoint(finalVertex2);
	
	float edgeMag = finalEdgeNormal.Magnitude();
	if( edgeMag > 0.0)
		translationalAxis = !finalEdgeNormal;
	float velocityProjection1 = translationalAxis.dot(velocityAtPoint1);
	float velocityProjection2 = translationalAxis.dot(velocityAtPoint2);
	
	
	Vector2D finalVertexWorldSpace = finalVertex + vertexOwner->position;
	Vector2D finalEdgeDirection = finalEdge[1] - finalEdge[0];

	finalEdgeDirection = !finalEdgeDirection;
	
	
	//translationalAxis = !edgeNormal;
		
	//minIntervalDistance *= 2.0;
	
	Vector2D collisionPoint = finalCollisionPoint;
	Vector2D edgeNormal = finalEdgeNormal;
	Vector2D edgeNormalDir = !edgeNormal;
	edgeNormal = edgeNormalDir * minIntervalDistance;
	//float returnPathVelocity1 = edgeNormalDir.(
	//float upDir = Vector2D(0,1).dot(edgeNormalDir);	
	if(edgeMag > 0.0)
	{
		if(((abs(velocityProjection1)>abs(velocityProjection2)  )|| edgeOwner->isPinned || edgeOwner->cantShift) && !(vertexOwner->isPinned) && !(vertexOwner->cantShift))
		{
			
			/*
			if(velocityProjection1 > 0)
			{
				printf("WHY DID THIS HAPPEN? velocityProjection = %f\n", velocityProjection1);
				std::cin.get();
			}
			else
				printf("velocityProjection = %f\n", velocityProjection1);
			*/
			if( vertexOwner->objectId == PINNEDOBJECT_CLASS)
			{
				printf("Rotating object1!\n");
				PinnedObject * po = (PinnedObject *)vertexOwner;
				po->RotateToEdge(finalVertex, finalEdge);
			}
			else
			{
				float mag = edgeNormal.Magnitude();
				if(mag > 0.0)
				{
					vertexOwner->position = vertexOwner->position + edgeNormal;//oppositeVelocity * minIntervalDistance;
					//vertexOwner->velocity = vertexOwner->velocity + edgeNormal/lastElapsedTime;
				}
			}
		}
		else
		{
			if( edgeOwner->objectId == PINNEDOBJECT_CLASS)
			{
				printf("Rotating object2!\n");
				PinnedObject * po = (PinnedObject *)edgeOwner;
				po->RotateToVertex(finalVertex2, finalEdge);
	
			}
			else
			{
				Vector2D oppositeVelocity = -1.0 * velocityProjection2 * translationalAxis;//edgeNormal;//-lineProjection * translationalAxis;
				if(oppositeVelocity.Magnitude() > 0.0)
				{
					oppositeVelocity = !oppositeVelocity;
					//edgeOwner->collisionPoint = edgeNormal * 100.0;//* minIntervalDistance;
					//edgeOwner->collisionValid = true;
					edgeOwner->position = edgeOwner->position - edgeNormal;//+ oppositeVelocity * minIntervalDistance;
					//edgeOwner->velocity = edgeOwner->velocity - edgeNormal/lastElapsedTime;
					//printf("new position = %f, %f\n", edgeOwner->position.x, edgeOwner->position.y);
				}
			}
		}
	}
	
	//vertexOwner->isPinned = true;
	//edgeOwner->isPinned = true;
	
	float massSum = vertexOwner->mass + edgeOwner->mass;

	float velocityChange1 = 0.0;
	float velocityChange2 = 0.0;
	float bounce = (-1.0 - edgeOwner->material->bounciness - vertexOwner->material->bounciness);
	if(edgeOwner->isPinned)
	{
		velocityChange1 = bounce * ( velocityProjection1 );
	}
	else
	{
		//velocityChange1 = bounce * (velocityProjection1 * vertexOwner->mass - velocityProjection2 * edgeOwner->mass)/massSum;
		velocityChange1 = bounce * (velocityProjection1 - velocityProjection2) * (edgeOwner->mass/massSum);
	}
	if(vertexOwner->isPinned)
	{
		velocityChange2 = bounce * ( velocityProjection2 );
	}
	else
	{
		velocityChange2 = bounce * (velocityProjection2 - velocityProjection1) * (vertexOwner->mass/massSum);
		//velocityChange2 = bounce * (velocityProjection2 * edgeOwner->mass - velocityProjection1 * vertexOwner->mass)/massSum;
	}
	
	Vector2D force1 = (velocityChange1 * vertexOwner->mass * translationalAxis)/elapsedTime;
	Vector2D force2 = (velocityChange2 * edgeOwner->mass * translationalAxis)/elapsedTime;
	
	vertexOwner->collisionPoint =  finalVertex;//force1;//collisionPoint - vertexOwner->position;
	vertexOwner->collisionValid = true;
	edgeOwner->collisionPoint = finalVertex2;//force2;//collisionPoint - edgeOwner->position;
	edgeOwner->collisionValid = true;
	vertexOwner->AddForceObjectSpace(force1, finalVertex);
	edgeOwner->AddForceObjectSpace(force2, finalVertex2);
	LineSegment collisionEdge = LineSegment(finalEdge[0], finalEdge[1], edgeOwner->material->friction + vertexOwner->material->friction);

	vertexOwner->AddContactPointObjectSpace(force2, finalVertex, collisionEdge, velocityAtPoint2);
	edgeOwner->AddContactPointObjectSpace(force1, finalVertex2, collisionEdge, velocityAtPoint1);
	
	float power = force1.Magnitude();
	
	edgeOwner->isColliding = true;
	vertexOwner->isColliding = true;
	edgeOwner->CollidedWith(vertexOwner, power, finalVertex2);
	vertexOwner->CollidedWith(edgeOwner, power, finalVertex);
	
	return true;	
}
void Object::CollidedWith(Object * object2, float power, Vector2D finalVertex)
{
	//we can make decisions here for special objects (arrow, grenade, etc).
}
Object * Object::CreateObject()
{
	Object * create = objectToCreate;
	objectToCreate = NULL;
	return create;
	
}
Object * Object::DestroyObject()
{
	Object * destroy = objectToDestroy;
	objectToDestroy = NULL;
	return destroy;
	
}
Vector2D Object::VelocityAtPoint(Vector2D point)
{
	float tangantialVelocity = point.Magnitude() * angularVelocity;
	Vector2D dir = Vector2D(-point.y, point.x);
	if(dir.Magnitude() > 0.0)
	{
		return !dir * tangantialVelocity + velocity;
	}
	return velocity;
}
void Object::ProjectPolygon(Vector2D axis, float * min, float * max, Vector2D * minVertex, Vector2D * maxVertex, float * minError, float * maxError)
{

	//make sure the axis points in the +x and +y directions
	float xDir = Vector2D(1,0).dot(axis);
	float yDir = Vector2D(0,1).dot(axis);
	if(abs(xDir)>abs(yDir))
	{
		if(xDir < 0)
			axis = -1.0 * axis;
	}
	else
	{
		if(yDir < 0)
			axis = -1.0 * axis;
	}
			
	float dotProduct = axis.dot(vertices[0] + position);
	*min = dotProduct;
	*max = dotProduct;
	*minVertex = vertices[0];
	*maxVertex = vertices[0];
	int minCount = 1;
	int maxCount = 1;
	*minError = 0.0;
	*maxError = 0.0;
	int minErrorCount = 0;
	int maxErrorCount = 0;
	for(int i = 1; i < numVertices; i++)
	{
		dotProduct = axis.dot(vertices[i] + position);
		if(dotProduct < *min)
		{
			minCount = 1;
			minErrorCount = 0;
			*minError = 0.0;
			*min = dotProduct;
			*minVertex = vertices[i];
		}
		else if( dotProduct == *min)
		{
			*minError += vertices[i].dist(*minVertex);
			*minVertex = *minVertex + vertices[i];
			minCount++;
			minErrorCount++;
			
		}
		else if(dotProduct > *max)
		{
			maxCount = 1;
			maxErrorCount = 0;
			*maxError = 0.0;
			*max = dotProduct;
			*maxVertex = vertices[i];
		}
		else if(dotProduct == *max)
		{
			*maxError += vertices[i].dist(*maxVertex);
			*maxVertex = *maxVertex + vertices[i];
			maxCount++;
		}
	}
	if(minErrorCount > 0)
		*minError /= (float)minErrorCount;
	if(maxErrorCount > 0)
		*maxError /= (float)maxErrorCount;
	*minVertex = *minVertex / (float)minCount;
	*maxVertex = *maxVertex / (float)maxCount;
} 
void Object::AddForce(Vector2D ray)
{	
	//it only affects translational momentum
	translationalForce = translationalForce + ray;
}
void Object::AddForce(Force force)
{
	AddForce(force.ray, force.origin);
}
void Object::AddContactPointObjectSpace(Vector2D normalForce, Vector2D contactPoint, LineSegment lineSegment)
{
	AddContactPointObjectSpace(normalForce,contactPoint, lineSegment, Vector2D(0,0));
}
void Object::AddContactPointObjectSpace(Vector2D normalForce, Vector2D contactPoint, LineSegment lineSegment, Vector2D otherVelocity)
{
	
	//just store the friction info in the objects list
	//we can't take care of it until we know all of the forces acting on the body
	
	//get the object's relative velocity in the direction of the segment

	float lineVelocity1 = lineSegment.direction.dot(VelocityAtPoint(contactPoint));
	float lineVelocity2 = lineSegment.direction.dot(otherVelocity);
	float lineVelocity = (lineVelocity1 - lineVelocity2);
	
	FrictionInfo * friction = new FrictionInfo(  normalForce, contactPoint, lineSegment.direction, lineVelocity, 500.0, 50.0, 0.3);
	
	if(!frictionInfoList)
		frictionInfoList = new FrictionInfoList(friction);
	else
		frictionInfoList->Add(friction);

	

	//if I am not moving along the friction axis, I need to apply a static frictional force that is equal to the
	//force opposing it until a maximum static frictional force has been reached.
	
	//if I am moving, I need to apply a frictional force proportional to the magnitude of my normal force,
	//but along the friction axis, in the antidirection of my velocity
	

	/*if(abs(lineVelocity) > abs(lineSegment.maxStaticFrictionalForce))
	{//printf("CONTROLLED OBJECT!!!!!!!!!\n");
	//printf("origin = (%f, %f)\n", origin.x, origin.y);
		force = lineSegment.direction * lineSegment.maxStaticFrictionalForce;
		if(lineVelocity > 0)
			force = -1.0 * force;
	}
	else
	{
		force = -1.0 * lineVelocity * lineSegment.direction;
	}
		
	AddForceObjectSpace(force, contactPoint, false);*/
}
void Object::AddForceObjectSpace(Vector2D ray, Vector2D origin, bool groundForce)
{
	AddForceObjectSpace(ray, origin);
}
void Object::AddForceObjectSpace(Vector2D ray, Vector2D origin)
{
	if(isPinned)
		return;
	if(ray.Magnitude() == 0.0)
		return;
	////printf("ray = (%f, %f)\n",ray.x, ray.y);
	////printf("origin = (%f, %f)\n",origin.x, origin.y);
	Vector2D towardCenter = -1.0 * origin;
	Vector2D unitTowardCenter = !towardCenter; 
	Vector2D unitRay = !ray;
	
	
	float cosTheta = unitTowardCenter.dot(ray);//unitTowardCenter.dot(ray);
	
	Vector2D rayTowardCenter = (cosTheta) * unitTowardCenter;
	AddForce(rayTowardCenter);

	Vector2D rayRotational = (ray - rayTowardCenter);
	
	
	torque += rayRotational.x * towardCenter.y - rayRotational.y * towardCenter.x;
}
void Object::AddForce(Vector2D ray, Vector2D origin)
{
	if(ray.Magnitude() == 0.0)
		return;
	if(origin.dist(position) < 0.000001)
	{
		AddForce(ray);
		return;
	}
	//printf("ray = (%f, %f)\n",ray.x, ray.y);
	//printf("origin = (%f, %f)\n",origin.x, origin.y);
	//adds a force that has some component that will cause translational motion
	//and some component that will cause rotational motion
	Vector2D towardCenter = position - origin;
	
	Vector2D unitTowardCenter = !towardCenter; 
	Vector2D unitRay = !ray;
	
	double cosTheta = unitTowardCenter * unitRay;
	
	Vector2D rayTowardCenter = (cosTheta * ray.Magnitude()) * unitTowardCenter;
	AddForce(rayTowardCenter);

	Vector2D rayRotational = ray - rayTowardCenter;
	
	torque += rayRotational.x * towardCenter.y - rayRotational.y * towardCenter.x;
}

void Object::AddGravity()
{
	Vector2D gravitationalForce = Vector2D(0.0,GRAVITATIONAL_FORCE) * mass;///(float)numVertices;
	AddForce(gravitationalForce);
	//gravitationalForce = gravitationalForce / ((float)numVertices );
	//for(int i = 0; i < numVertices; i++)
	//	AddForceObjectSpace(gravitationalForce, vertices[i]);
}
void Object::ThinkEntryPoint(double elapsedTime)
{
		translationalForce = Vector2D(0,0);
		rotation = angularVelocity * elapsedTime;
		while(rotation > 2 * 3.14159)
			rotation -= 2 * 3.14159;
		Rotate(rotation);
}
char * Object::Action(Vector2D playerPos, Dictionary * gameState)
{
	return NULL;
}
bool Object::GetCollideWithGround() const
{
	return collideWithGround;
}
